using System;
using Geometry;
using Unity.Mathematics;
namespace EarthGeospatial
{
    public class OrientedBoundingBox
    {
        public double3 _center;
        public double3x3 _halfAxes;


        public OrientedBoundingBox(double3 center, double3x3 halfAxes)
        {
            this._center = center;
            this._halfAxes = halfAxes;
        }

        public static implicit operator bool(OrientedBoundingBox self)
        {
            return self == null ? false : true;
        }

        public CullingResult intersectPlane(Plane plane)
        {
            double3 normal = plane.Normal;

            double3x3 halfAxes = this._halfAxes;
            double3 xAxisDirectionAndHalfLength = halfAxes[0];
            double3 yAxisDirectionAndHalfLength = halfAxes[1];
            double3 zAxisDirectionAndHalfLength = halfAxes[2];

            // plane is used as if it is its normal; the first three components are
            // assumed to be normalized
            double radEffective = math.abs(
                                           normal.x * xAxisDirectionAndHalfLength.x +
                                           normal.y * xAxisDirectionAndHalfLength.y +
                                           normal.z * xAxisDirectionAndHalfLength.z) +
                                       math.abs(
                                           normal.x * yAxisDirectionAndHalfLength.x +
                                           normal.y * yAxisDirectionAndHalfLength.y +
                                           normal.z * yAxisDirectionAndHalfLength.z) +
                                       math.abs(
                                           normal.x * zAxisDirectionAndHalfLength.x +
                                           normal.y * zAxisDirectionAndHalfLength.y +
                                           normal.z * zAxisDirectionAndHalfLength.z);

            double distanceToPlane = math.dot(normal, this._center) + plane.Distance;

            if (distanceToPlane <= -radEffective)
            {
                // The entire box is on the negative side of the plane normal
                return CullingResult.Outside;
            }
            if (distanceToPlane >= radEffective)
            {
                // The entire box is on the positive side of the plane normal
                return CullingResult.Inside;
            }
            return CullingResult.Intersecting;
        }


        public double computeDistanceSquaredToPosition(double3 position)
        {
            double3 offset = position - this._center;

            double3x3 halfAxes = this._halfAxes;
            double3 u = halfAxes[0];
            double3 v = halfAxes[1];
            double3 w = halfAxes[2];

            double uHalf = math.length(u);
            double vHalf = math.length(v);
            double wHalf = math.length(w);

            u /= uHalf;
            v /= vHalf;
            w /= wHalf;

            double3 pPrime = new double3(
                 math.dot(offset, u),
                 math.dot(offset, v),
                 math.dot(offset, w));

            double distanceSquared = 0.0;
            double d;

            if (pPrime.x < -uHalf)
            {
                d = pPrime.x + uHalf;
                distanceSquared += d * d;
            }
            else if (pPrime.x > uHalf)
            {
                d = pPrime.x - uHalf;
                distanceSquared += d * d;
            }

            if (pPrime.y < -vHalf)
            {
                d = pPrime.y + vHalf;
                distanceSquared += d * d;
            }
            else if (pPrime.y > vHalf)
            {
                d = pPrime.y - vHalf;
                distanceSquared += d * d;
            }

            if (pPrime.z < -wHalf)
            {
                d = pPrime.z + wHalf;
                distanceSquared += d * d;
            }
            else if (pPrime.z > wHalf)
            {
                d = pPrime.z - wHalf;
                distanceSquared += d * d;
            }

            return distanceSquared;
        }
    }
}